/**
 * @file: TrackBase.cpp
 * @brief: track类抽象，存放属于此轨迹的装甲板，轨迹滤波器和所属车辆等信息。
 * @author zhang hanhsin (2410748989@qq.com)
 * @date : 2024-03-29
 *
 * @copyright Copyright (C) 2024, HITCRT_VISION, all rights reserved.
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Author  <th>Description
 * <tr><td>2024-03-29 <td>zhang hanhsin  <td>
 * </table>
 */
#include "TrackBase.h"

namespace hitcrt {

Track::Track(uint maxArmorSize) {
    // 编号
    m_pattern = (Pattern)0;
    // 装甲板总数
    m_total = 0;
    m_errCnt = 0;
    // 轨迹信息
    m_missCnt = 0;
    m_findCnt = 0;
    m_spinCnt = 0;
    m_isUpdated = false;
    m_isGoal = false;
    m_resetFlag = false;
    m_afterResetCnt = 0;
    m_trjState = TrjState::INIT;
    m_maxArmorSize = maxArmorSize;
    m_foundNums = std::vector<int>(12, 0);
    // m_AKFPreNum = 0;
    // TODO Thresh加入参数表
    // 对于第二块匹配的装甲板 需要给一个出事阈值判断他的距离是否合适
    // 所以一定要给一个初始值
    m_distanceThresh = 2;
    m_missCntThresh = 20;
    m_clearThresh = 200;

    // bool armorOnly = true;
    // std::shared_ptr<EKFCar> tmpAEKFCarontainer;
    // tmpAEKFCarontainer = KalmanFactory::createEKFCar(armorOnly);
    // m_trjAKF = tmpAEKFCarontainer;

    int stateDimension = 6;  // 这里使用的是匀速直线运动模型 所以维度为6
    m_filtMatrix.resize(stateDimension, 1);
}
void Track::reset(const Armor &armor, const TimePoint detectTime) {
    // 编号
    m_pattern = (Pattern)0;
    // 装甲板总数
    m_total = 0;
    m_errCnt = 0;
    // 轨迹信息
    m_missCnt = 0;
    m_findCnt = 0;
    m_spinCnt = 0;
    m_isUpdated = false;
    m_isGoal = false;
    m_resetFlag = false;
    m_afterResetCnt = 0;
    m_trjState = TrjState::RESET;
    m_armors.push(armor);
    // m_AKFPreNum = 0;
    m_foundNums[armor.m_pattern] += 1;

    m_createTime = detectTime;

    // 在每个轨迹创建的时候对成员  <kalmanFilter>m_kf1 初始化
    // kalmanFilter 初始化是塞进去一个点

    if (m_armors.size() > 0) {
        m_kf1 = std::make_shared<hitcrt::KalmanFilter>(cv::Point3f(
            m_armors.back().m_measurePoint.x, m_armors.back().m_measurePoint.y,
            m_armors.back().m_measurePoint.z));
        // 这里打包初始化用到的 噪声参数
        std::vector<boost::any> param;
        param.push_back(std::vector<double>(0.001));
        m_kf1->init(param);  // 大的滤波器初始化 ——> 对每个XYZ滤波器初始化
        // std::cout << "测试用" << std::endl;
        // std::cout << m_kf1->m_filter[0].getResult() << std::endl;
        // std::cout << m_kf1->m_filter[1].getResult() << std::endl;
        // std::cout << m_kf1->m_filter[2].getResult() << std::endl;
        // getchar();
    }
    // m_residualUVFiltShort = 0.0;
    // m_residualUVFiltLong = 1.0;
}
void Track::clear() {
    // 编号
    m_pattern = (Pattern)0;
    // 装甲板总数
    m_total = 0;
    // 轨迹信息
    m_missCnt = 0;
    m_findCnt = 0;
    m_errCnt = 0;
    m_spinCnt = 0;
    m_isUpdated = false;
    m_isGoal = false;
    m_resetFlag = false;
    m_afterResetCnt = 0;
    m_trjState = TrjState::INIT;
    // m_AKFPreNum = 0;
    m_foundNums = std::vector<int>(12, 0);
    std::queue<Armor>().swap(m_armors);
}
// 更新此次识别的时间，不管有没有装甲板都要做，因为滤波时要用
void Track::updateDetectTime(const TimePoint detectTime) {
    m_lastDetectTime = m_thisDetectTime;
    m_thisDetectTime = detectTime;
}
std::chrono::nanoseconds Track::getDetectInterval() {
    std::chrono::nanoseconds interval =
        std::chrono::duration_cast<std::chrono::nanoseconds>(m_thisDetectTime -
                                                             m_lastDetectTime);
    return interval > std::chrono::nanoseconds{0} ? interval
                                                  : std::chrono::nanoseconds{0};
}

void Track::kalamnfliter() {}

}  // namespace hitcrt
